#include "ptzcontrol.h"
#include "timestamp.h"

PtzControl::PtzControl()
{
    this->m_ok = false;


}

PtzControl::~PtzControl(){
    printf("ptz closed\t");
    TimeStamp::now()->print();
}

void PtzControl::initial(){
    char * tagf = "ttyACM";
    this->m_ok = true;
    if(!m_ptz.OpenSerial(tagf, 0, 115200, 8, 1, 1)){
        if(!m_ptz.OpenSerial(tagf, 1, 115200, 8, 1, 1)){
            perror("ptz error: serail port");
            this->m_ok = false;
        }
    }
    if(this->m_ok){
        if(!m_ptz.StartSerial()){
            perror("ptz error: can't receive serial port");
            this->m_ok = false;
            return;
        }
        memset(m_command, 0, 20);
        int value = 0 ;
        m_ptz.SetPwmToStr(0, value, m_command);
        m_ptz.SendData(m_command, 12);
        m_ptz.SetPwmToStr(1, value, m_command);
        m_ptz.SendData(m_command, 12);
        m_ptz.SetPwmToStr(2, value, m_command);
        m_ptz.SendData(m_command, 12);
    }


}

void PtzControl::set(int x, int y, int z){
    if(this->m_ok){
        memset(m_command, 0, 20);
        if(m_ptz.SetPwmToStr(0, x, m_command))
            m_ptz.SendData(m_command, 12);
        memset(m_command, 0, 20);
        if(m_ptz.SetPwmToStr(1, y, m_command))
            m_ptz.SendData(m_command, 12);
        memset(m_command, 0, 20);
        if(m_ptz.SetPwmToStr(2, z, m_command))
            m_ptz.SendData(m_command, 12);
    }
}

bool PtzControl::get(int &x, int &y, int &z){
     x = m_ptz.getValue(0);
     y = m_ptz.getValue(1);
     z = m_ptz.getValue(2);
     if(x > 360 || x < 0 ||
         y > 360 || y < 0||
         z > 360 || z < 0){
         x = 0;
         y = 0;
         z = 0;
     }
    return true;
}

